#include "hapr_assmt_linefollow.h"
#include "debug_frmwrk.h"
#include <stdlib.h>

#define DEBUG 1

int main(void){
  

  LineFollowCfg cfg;
  cfg.FollowSpeed = 0x1F;
  cfg.P_Factor = 0x01;     //Perceived displacement from line
  cfg.P_Quotient = 0x14;    
  cfg.D_Factor = 0x03;     //Rate of correction
  cfg.D_Quotient = 0x02; 

  LINE_SerialInit();

  SensorVals s;
  //  LINE_AutoCalibrate();
  LINE_GetSensors(&s);
  printSensors(&s);

  LINE_AutoCalibrate();
  LINE_StartFollow(&cfg);
  while (s.PC0 > 100 ||
	 s.PC1 > 100 ||
	 s.PC2 > 100 ||
	 s.PC3 > 100 ||
	 s.PC4 > 100)
    LINE_GetSensors(&s);
    printSensors(&s);
  LINE_EndFollow(0x1F);
  
  
  
  
  while(1){
    //  LINE_GetSensors(&s);
    //  printSensors(&s);
};

  return 0;
}

#if DEBUG
void printSensors(SensorVals* v){
  debug_frmwrk_init();
  char*  msg[6];
  _DBG_((v->status)?"CALIBRATED":"RAW");

    sprintf(msg, "%d -- %x", v->PC0, v->PC0);
  _DBG_(msg);
  sprintf(msg, "%d -- %x", v->PC1, v->PC1);
  _DBG_(msg);
  sprintf(msg, "%d -- %x", v->PC2, v->PC2);
  _DBG_(msg);
  sprintf(msg, "%d -- %x", v->PC3, v->PC3);
  _DBG_(msg);
  sprintf(msg, "%d -- %x", v->PC4, v->PC4);
  _DBG_(msg);
}
#endif
